#include <cstdio>
#include "FaceModel.h"
#include "SimpleVectorMatrix.h"
#include <GL/glut.h>
#include <GL/gl.h>


const double MAX_DISTANCE = 20;


FaceModel faceModel;


FaceModel::FaceModel()
{
	clearModel();

	mCenterPoint.x = 0;
	mCenterPoint.y = 0;
	mCenterPoint.z = 0;
}


FaceModel::~FaceModel()
{
}


void FaceModel::applyTransformMatrix(FaceParts face_part)
{
	switch (face_part) {
	case EYES_LINE:
		glTranslatef((mExpectedPoints[0].x + mExpectedPoints[1].x), 0.5f * (mExpectedPoints[0].y + mExpectedPoints[1].y)+0.5f, 0.6f);
		break;
	}
}


void FaceModel::clearModel()
{
	mExpectedPoints.clear();

	mFramesSaved = 1;

	// under left eye
	Point3D pLeftEye;
	pLeftEye.x = -0.85f;
	pLeftEye.y = -1.5f;
	pLeftEye.z =  0.8f;
	mExpectedPoints.push_back(pLeftEye);

	// under right eye
	Point3D pRightEye;
	pRightEye.x = -pLeftEye.x;
	pRightEye.y =  pLeftEye.y;
	pRightEye.z =  pLeftEye.z;
	mExpectedPoints.push_back(pRightEye);

	// nose
	//Point3D pNose;
	//pNose.x =  0.0f;
	//pNose.y = -1.8f;
	//pNose.z =  0.2f;
	//mExpectedPoints.push_back(pNose);
}

void FaceModel::updateModel()
{
	Point3D center3d = mCenterPoint;
	Point center2d = convert3DTo2D(mCenterPoint);

	for (int i=0; i<mExpectedPoints.size(); ++i)
	{
		double best_distance = MAX_DISTANCE;
		int best_marker_id = -1;

		Point3D ep3d = mExpectedPoints[i];
		Point ep2d = convert3DTo2D(ep3d);

		Point3D dist_ep3d_center = distance_v(ep3d, center3d);
		Point dist_ep2d_center = distance_v(ep2d, center2d);
		
		for (int j=0; j<blueMarkers.size(); ++j)
		{
			Point marker = blueMarkers[j];
			marker.y = 480 - marker.y;
			double d = distance(ep2d, marker);
			if (d < best_distance)
			{
				best_distance = d;
				best_marker_id = j;
			}
		}

		if (best_marker_id > -1) 
		{
			printf("Point %d - (%f,%f)\n", i, ep2d.x, ep2d.y);

			Point marker = blueMarkers[best_marker_id];
			marker.y = 480 - marker.y;
			Point dist = distance_v(ep2d, marker);

			Point3D correction;
			correction.x = - dist.x * dist_ep3d_center.x / dist_ep2d_center.x;
			correction.y = - dist.y * dist_ep3d_center.y / dist_ep2d_center.y;
			correction.z = 0;

			correct(ep3d, correction, mFramesSaved++);
			mExpectedPoints[i] = ep3d;
		}
	}
}

void FaceModel::displayModelPoints()
{
	// This is VERY stange and needs REFACTORING!!!!!
	std::vector<Point> points;
	for (int i=0; i<mExpectedPoints.size(); ++i)
		points.push_back(convert3DTo2D(mExpectedPoints[i]));

	for (int i=0; i<mExpectedPoints.size(); ++i)
		drawPoint2D(points[i]);
}

void FaceModel::drawPoint2D(Point &p)
{
	glDisable( GL_DEPTH_TEST );
	glMatrixMode( GL_PROJECTION );
	glPushMatrix();
	glLoadIdentity();
	gluOrtho2D( 0.0, 640, 0.0, 480 );

	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();

	glLoadIdentity();
	glTranslatef(p.x, p.y, 10);
	glutSolidSphere(10, 10, 10);

	glPopMatrix();


	glMatrixMode( GL_PROJECTION );
	glPopMatrix();
	glEnable(GL_DEPTH_TEST);
}

Point FaceModel::convert3DTo2D(Point3D &p)
{
	GLdouble proj_matrix[16];				
	glGetDoublev(GL_PROJECTION_MATRIX, proj_matrix);	
	
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	glTranslatef(p.x, p.y, p.z);
	GLdouble mod_matrix[16];				
	glGetDoublev(GL_MODELVIEW_MATRIX, mod_matrix);	
	glPopMatrix();

	GLint view_matrix[4];
	glGetIntegerv(GL_VIEWPORT, view_matrix);

	Point p2d;
	double t;
	gluProject(0, 0, 0, mod_matrix, proj_matrix, view_matrix, &p2d.x, &p2d.y, &t);

	//printf("%f, %f\n", p2d.x, p2d.y);

	return p2d;
}
